// Interrupts are generated every 20ms
// Modulo division by 5 causes each time slot to happen every 100ms

void interruptHandler() {
  switch ((timeSlot += 1) % 5) {
  case 0:
    // Read Receiver
    roll = pulseIn(ROLLPIN, HIGH, TIMEOUT);
    aux = pulseIn(AUXPIN, HIGH, TIMEOUT);
    pitch = pulseIn(PITCHPIN, HIGH, TIMEOUT);
    yaw = pulseIn(YAWPIN, HIGH, TIMEOUT);
    throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT);
    gear = pulseIn(GEARPIN, HIGH, TIMEOUT);
    break;
  case 1:
    // Check Query
    if (Serial.available())
      queryType = Serial.read();
    break;
  case 2:
    // Send Data
    switch (queryType) {
    case'G':
      Serial.print(deltaTime);
      Serial.print(',');
      Serial.print(rollRate);
      Serial.print(',');
      Serial.print(pitchRate);
      Serial.print(',');
      Serial.println(yawRate);
      break;
    case 'P':
      Serial.print(deltaTime);
      Serial.print(',');
      Serial.print(int(pgain*1000));
      Serial.print(',');
      Serial.print(int(igain*1000));
      Serial.print(',');
      Serial.println(int(dgain*1000));
      break;
    }
  }
}
